#include "Chassis.h"
#include "myInit.h"

#define Lx 1
#define Ly 1 

struct DirectionControl 
{
    float Angle_1, Angle_2, Angle_3, Angle_4;  
											   
    int8_t sign;  
};

static float Angle_Min(float X, float Y) 
{
    float Z1 = Y - X;
    while(Z1 < -180.0f) 
		Z1 += 360;
    while(Z1 > 180.0f)
		Z1 -= 360;
    return Z1;
}

static float Angle_Optimize(float Z) 
{
    if (Z > 90) return -(180 - Z);
    else if (Z < -90) return 180 + Z;
    return Z;
}

void ControlDirection(struct DirectionControl *ctrl, float* M, int16_t* N) 
{
	ctrl->Angle_4 = Angle_Min(ctrl->Angle_2, *M);  
	ctrl->Angle_3 = ctrl->Angle_1 + Angle_Optimize(ctrl->Angle_4);  
	
	if (ctrl->Angle_4 > 90.0f || ctrl->Angle_4 < -90.0f)  
		ctrl->sign = -(ctrl->sign);  
	
	ctrl->Angle_1 = ctrl->Angle_3; 
	ctrl->Angle_2 = *M;  
	
	if (ctrl->sign==-1)  
		*N = -*N;  
	
	*M = ctrl->Angle_3;  
}

struct DirectionControl Wheel_1 = {0, 0, 0, 0, 1};
struct DirectionControl Wheel_2 = {0, 0, 0, 0, 1};
struct DirectionControl Wheel_3 = {0, 0, 0, 0, 1};
struct DirectionControl Wheel_4 = {0, 0, 0, 0, 1};

void M_dir_ct_Four_plus(float *M_LB_A, int16_t *M_LB_V,
                        float *M_LF_A, int16_t *M_LF_V,
                        float *M_RB_A, int16_t *M_RB_V,
                        float *M_RF_A, int16_t *M_RF_V) 
{
    ControlDirection(&Wheel_1, M_LB_A, M_LB_V);
    ControlDirection(&Wheel_2, M_LF_A, M_LF_V);
    ControlDirection(&Wheel_3, M_RB_A, M_RB_V);
    ControlDirection(&Wheel_4, M_RF_A, M_RF_V);
}

void Delay(uint32_t Delaytime)
{
	osDelay(Delaytime);
}

/*------计算自转速度------*/
#define Kp 6
#define Kd 0.1

float Calculate_W(float ComputerAng_DIFF,float CoordAng_DIFF,float sYaw_Aim
					,float sYaw_Ang,uint8_t W_Sign) 
{
	static float sW = 0;
	
	static float CoordAng_DIFF_Next = 0;
	static float CoordAng_DIFF_Last = 0;
	static float sW_Last = 0;
	
	static uint16_t sStopCount 	= 0;
	static char Pri[16]    		= {0};  // 打印的数组
	
	switch (W_Sign)
	{
		case  0:  	// 边走边瞄
					sW  = Kp*(CoordAng_DIFF - CoordAng_DIFF_Next)
							+Kd*(CoordAng_DIFF-2*CoordAng_DIFF_Next+CoordAng_DIFF_Last);
					CoordAng_DIFF_Last = CoordAng_DIFF_Next;
					CoordAng_DIFF_Next = CoordAng_DIFF;
					sW = sW_Last + sW;
					sW_Last = sW;
					break;
		
		case  1:	// 相机瞄准
					sW = 7*ComputerAng_DIFF;
					if(fabs(ComputerAng_DIFF)<3.5)
						sW =  3*ComputerAng_DIFF;
					if(ComputerAng_DIFF>0&&ComputerAng_DIFF<0.8)
					{
						sW =  3;
					}
					if(ComputerAng_DIFF<0&&ComputerAng_DIFF>-0.8)
					{
						sW = -3;
					}
					if(fabs(ComputerAng_DIFF) < 0.2)  // 相机死区
					{
						sW = 0;
					}	
					break; 	
					
		case  2:    // 雷达瞄准(单片机算)
					sW = 6*CoordAng_DIFF;
					if(CoordAng_DIFF>0&&CoordAng_DIFF< 0.8)
						sW =  3;
					if(CoordAng_DIFF<0&&CoordAng_DIFF>-0.8)
						sW = -3;
					if(fabs(CoordAng_DIFF) < 0.45)  // 雷达死区
						sW = 0.000001;
					break; 

		case  3:    // 雷达瞄准(小电脑算)
					sW = -5*ComputerAng_DIFF;
					if(ComputerAng_DIFF>0&&ComputerAng_DIFF<1)
						sW = -3;
					if(ComputerAng_DIFF<0&&ComputerAng_DIFF>-1)
						sW =  3;
					if(fabs(ComputerAng_DIFF) < 0.5)  // 
						sW = 0.000001;
					break; 

		case  4:    // 保持瞄好的角度
						sW = 0;
					break; 		
		
		case 5:     // 保持正方向
					sW = -5*sYaw_Ang;
					if(sYaw_Ang>0&&sYaw_Ang< 1)
						sW =  -3;
					if(sYaw_Ang<0&&sYaw_Ang>-1)
						sW =   3;
					if(fabs(sYaw_Ang) < 0.45)  // 雷达死区
						sW = 0.000001;
					break; 
					
		case 7:     sW = 0;	
					break;
					
		default:	break;	
	}
	
	if(sW>450)
		sW=450;
	if(sW<-450)
		sW=-450;
	
	return sW;
}

/*------底盘控制------*/
void Helm_Wheel_Control(int16_t Vx,int16_t Vy,float W,int8_t W_Sign,uint8_t sBlock_Sign)
{		
	static int16_t V_LF,V_LB,V_RF,V_RB = 0;
	
	static float angle_LF,angle_LB,angle_RF,angle_RB = 0.f;
	static float angle_LF_Last,angle_LB_Last,angle_RF_Last,angle_RB_Last = 0.f;
	
	static MotorToCAN sMotorToCAN;
	
	/*---解算---*/
	if( W ==-10 || W == 0 )
		W = 0.000001;
		
    angle_LF = atan2f((Vy+W*Lx),(Vx-W*Ly)); 
    angle_LB = atan2f((Vy-W*Lx),(Vx-W*Ly));
    angle_RF = atan2f((Vy+W*Lx),(Vx+W*Ly));
    angle_RB = atan2f((Vy-W*Lx),(Vx+W*Ly)); 
	 
	if(Vx==0 && Vy==0 && W_Sign==0)
	{
		W = 0;
		angle_LF = angle_LF_Last;
		angle_LB = angle_LB_Last;
		angle_RF = angle_RF_Last;
		angle_RB = angle_RB_Last;
	}else
	{
		angle_LF_Last = angle_LF;
		angle_LB_Last = angle_LB;
		angle_RF_Last = angle_RF;
		angle_RB_Last = angle_RB;
	}
	
    V_LF = sqrt((Vx-W*Ly)*(Vx-W*Ly)+(Vy+W*Lx)*(Vy+W*Lx));
    V_LB = sqrt((Vx-W*Ly)*(Vx-W*Ly)+(Vy-W*Lx)*(Vy-W*Lx));
    V_RF = sqrt((Vx+W*Ly)*(Vx+W*Ly)+(Vy+W*Lx)*(Vy+W*Lx));
    V_RB = sqrt((Vx+W*Ly)*(Vx+W*Ly)+(Vy-W*Lx)*(Vy-W*Lx)); 
	
	angle_LF = My_RadToAng(angle_LF);  
	angle_LB = My_RadToAng(angle_LB);
	angle_RF = My_RadToAng(angle_RF);
	angle_RB = My_RadToAng(angle_RB); 

	M_dir_ct_Four_plus(&angle_LF, &V_LF,
					   &angle_LB, &V_LB, 
					   &angle_RF, &V_RF,
					   &angle_RB, &V_RB);
	/*---驱动---*/
	sMotorToCAN.ID = HelmLFID;
	sMotorToCAN.Data = angle_LF;		//
	xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
	sMotorToCAN.ID = HelmRFID;
	sMotorToCAN.Data = angle_RF;		//
	xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
	sMotorToCAN.ID = HelmLBID;
	sMotorToCAN.Data = angle_LB;		//
	xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
	sMotorToCAN.ID = HelmRBID;
	sMotorToCAN.Data = angle_RB;		//
	xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
	osDelay(1);
	
	if(Vx==0 && Vy==0 && W_Sign==0 )
	{
		sMotorToCAN.ID = WheelLFID;
		sMotorToCAN.eMode = eMotorMode_Break;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
		sMotorToCAN.ID = WheelRFID;
		sMotorToCAN.eMode = eMotorMode_Break;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
		sMotorToCAN.ID = WheelLBID;
		sMotorToCAN.eMode = eMotorMode_Break;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
		sMotorToCAN.ID = WheelRBID;
		sMotorToCAN.eMode = eMotorMode_Break;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
	}
	else
	{
		sMotorToCAN.ID = WheelLFID;
		sMotorToCAN.Data =  -V_LF;		//
		sMotorToCAN.eMode = eMotorMode_Speed;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
		sMotorToCAN.ID = WheelRFID;
		sMotorToCAN.Data = -V_RF;		//
		sMotorToCAN.eMode = eMotorMode_Speed;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
		sMotorToCAN.ID = WheelLBID;
		sMotorToCAN.Data =  V_LB;		//
		sMotorToCAN.eMode = eMotorMode_Speed;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
		sMotorToCAN.ID = WheelRBID;
		sMotorToCAN.Data =  -V_RB;		//
		sMotorToCAN.eMode = eMotorMode_Speed;
		xQueueSend(other_CAN1Handle,&sMotorToCAN,portMAX_DELAY);
 	}
}
